/*
 * Multi-Node Communication Example
 * 
 * This example demonstrates multi-node communication using CDNET protocol.
 * It shows how to implement a simple sensor network with multiple devices.
 * 
 * Hardware connections:
 * - Use RS-485 transceivers for multi-node bus
 * - Connect all devices in parallel on the same bus
 * - Ensure proper termination resistors
 */

#include <SimpleCDNET.h>

// Create CDNET instance
SimpleCDNET cdnet;

// Device configuration
const uint8_t DEVICE_ADDRESS = 0x01;  // Change this for each device
const uint8_t MASTER_ADDRESS = 0x00;  // Master device address
const uint8_t SENSOR_PORT = 0x10;     // Port for sensor data
const uint8_t COMMAND_PORT = 0x20;    // Port for commands

// Device type identification
enum DeviceType {
    DEVICE_MASTER = 0x00,
    DEVICE_SENSOR = 0x01,
    DEVICE_ACTUATOR = 0x02
};

// Sensor data structure
struct SensorData {
    uint16_t temperature;  // Temperature in 0.01°C
    uint16_t humidity;     // Humidity in 0.01%
    uint16_t light;        // Light level (0-1023)
    uint8_t battery;        // Battery level (0-100%)
};

// Global variables
DeviceType deviceType = DEVICE_SENSOR;
uint32_t lastSensorRead = 0;
uint32_t lastBroadcast = 0;

void setup() {
    Serial.begin(115200);
    while (!Serial) {
        ; // wait for serial port to connect
    }
    
    Serial.println("Multi-Node CDNET Example");
    Serial.print("Device Address: 0x");
    Serial.println(DEVICE_ADDRESS, HEX);
    
    // Initialize CDNET
    cdnet.begin(&Serial1, DEVICE_ADDRESS);
    cdnet.setBaudRate(115200);
    
    // Configure device type based on address
    if (DEVICE_ADDRESS == 0x00) {
        deviceType = DEVICE_MASTER;
        Serial.println("Mode: MASTER");
    } else if (DEVICE_ADDRESS <= 0x0F) {
        deviceType = DEVICE_SENSOR;
        Serial.println("Mode: SENSOR");
    } else {
        deviceType = DEVICE_ACTUATOR;
        Serial.println("Mode: ACTUATOR");
    }
    
    Serial.println("Commands:");
    Serial.println("  's' - Send sensor data (sensors only)");
    Serial.println("  'p' - Ping all devices (master only)");
    Serial.println("  'd' - Discover devices (master only)");
    Serial.println("  'r' - Read received data");
}

void loop() {
    cdnet.update();  // Process incoming data
    
    // Handle different device behaviors
    switch (deviceType) {
        case DEVICE_MASTER:
            handleMaster();
            break;
            
        case DEVICE_SENSOR:
            handleSensor();
            break;
            
        case DEVICE_ACTUATOR:
            handleActuator();
            break;
    }
    
    // Process serial commands
    handleSerialCommands();
    
    // Process received data
    processReceivedData();
    
    delay(10);  // Small delay for stability
}

void handleMaster() {
    // Master periodically requests data from sensors
    static uint32_t lastRequest = 0;
    static uint8_t currentSensor = 0x01;
    
    if (millis() - lastRequest > 5000) {  // Request every 5 seconds
        // Send request to next sensor
        uint8_t request = 0x01;  // Request sensor data
        cdnet.sendL0(currentSensor, COMMAND_PORT, &request, 1);
        
        Serial.print("Requested data from sensor 0x");
        Serial.println(currentSensor, HEX);
        
        currentSensor++;
        if (currentSensor > 0x0F) {
            currentSensor = 0x01;
        }
        lastRequest = millis();
    }
}

void handleSensor() {
    // Sensors periodically broadcast their data
    if (millis() - lastBroadcast > 10000) {  // Broadcast every 10 seconds
        SensorData sensorData;
        
        // Simulate sensor readings
        sensorData.temperature = random(2000, 3000);  // 20-30°C
        sensorData.humidity = random(3000, 7000);     // 30-70%
        sensorData.light = random(0, 1023);         // Light level
        sensorData.battery = random(20, 100);         // Battery level
        
        // Send sensor data
        cdnet.sendL0(MASTER_ADDRESS, SENSOR_PORT, 
                    (uint8_t*)&sensorData, sizeof(sensorData));
        
        Serial.println("Broadcasted sensor data");
        lastBroadcast = millis();
    }
}

void handleActuator() {
    // Actuators wait for commands
    // Implementation depends on specific actuator
}

void handleSerialCommands() {
    if (Serial.available()) {
        char cmd = Serial.read();
        
        switch (cmd) {
            case 's':
                if (deviceType == DEVICE_SENSOR) {
                    forceSensorBroadcast();
                }
                break;
                
            case 'p':
                if (deviceType == DEVICE_MASTER) {
                    pingAllDevices();
                }
                break;
                
            case 'd':
                if (deviceType == DEVICE_MASTER) {
                    discoverDevices();
                }
                break;
                
            case 'r':
                readReceivedData();
                break;
        }
    }
}

void forceSensorBroadcast() {
    if (deviceType == DEVICE_SENSOR) {
        SensorData sensorData;
        sensorData.temperature = random(2000, 3000);
        sensorData.humidity = random(3000, 7000);
        sensorData.light = random(0, 1023);
        sensorData.battery = random(20, 100);
        
        cdnet.sendL0(MASTER_ADDRESS, SENSOR_PORT, 
                    (uint8_t*)&sensorData, sizeof(sensorData));
        
        Serial.println("Forced sensor broadcast");
    }
}

void pingAllDevices() {
    if (deviceType == DEVICE_MASTER) {
        Serial.println("Pinging all devices...");
        
        for (uint8_t addr = 0x01; addr <= 0xFF; addr++) {
            if (addr == DEVICE_ADDRESS) continue;
            
            uint8_t ping = 0x00;  // Ping command
            cdnet.sendL0(addr, COMMAND_PORT, &ping, 1);
            
            Serial.print("Pinged 0x");
            Serial.println(addr, HEX);
        }
    }
}

void discoverDevices() {
    if (deviceType == DEVICE_MASTER) {
        Serial.println("Discovering devices...");
        
        uint8_t discover = 0xFF;  // Discovery broadcast
        cdnet.sendL0(CDBUS_BROADCAST_ADDR, COMMAND_PORT, &discover, 1);
    }
}

void processReceivedData() {
    uint8_t src;
    uint8_t port8;
    uint16_t port16;
    uint8_t buffer[64];
    uint8_t len;
    
    // Process L0 packets
    len = cdnet.readL0(&src, &port8, buffer, sizeof(buffer));
    if (len > 0) {
        handleL0Packet(src, port8, buffer, len);
    }
    
    // Process L1 packets
    len = cdnet.readL1(&src, &port16, buffer, sizeof(buffer));
    if (len > 0) {
        handleL1Packet(src, port16, buffer, len);
    }
}

void handleL0Packet(uint8_t src, uint8_t port, uint8_t *data, uint8_t len) {
    switch (port) {
        case SENSOR_PORT:
            if (deviceType == DEVICE_MASTER && len >= sizeof(SensorData)) {
                SensorData *sensorData = (SensorData*)data;
                
                Serial.print("Sensor 0x");
                Serial.print(src, HEX);
                Serial.print(" - Temp: ");
                Serial.print(sensorData->temperature / 100.0);
                Serial.print("°C, Humidity: ");
                Serial.print(sensorData->humidity / 100.0);
                Serial.print("%, Light: ");
                Serial.print(sensorData->light);
                Serial.print(", Battery: ");
                Serial.print(sensorData->battery);
                Serial.println("%");
            }
            break;
            
        case COMMAND_PORT:
            if (deviceType == DEVICE_SENSOR) {
                if (len > 0 && data[0] == 0x01) {  // Request data
                    forceSensorBroadcast();
                }
            }
            break;
    }
}

void handleL1Packet(uint8_t src, uint16_t port, uint8_t *data, uint8_t len) {
    // Handle Layer 1 packets
    Serial.print("L1 Packet from 0x");
    Serial.print(src, HEX);
    Serial.print(" port 0x");
    Serial.print(port, HEX);
    Serial.print(" len ");
    Serial.println(len);
}